#include "opencv2/video/tracking.hpp"
#include "opencv2/imgproc/imgproc.hpp"
#include "opencv2/highgui/highgui.hpp"
#include "opencv2/calib3d/calib3d.hpp"
#include <fstream>
#include <iostream>
#include "DJI_guidance.h"
#include "DJI_utility.h"

#include <cstring>
#include <vector>
#include <list>
#include <sys/time.h>


// OpenCV library for easy access to USB camera and drawing of images
// on screen
#include "opencv2/opencv.hpp"

// April tags detector and various families that can be selected by command line option
#include "AprilTags/TagDetector.h"
#include "AprilTags/Tag16h5.h"
#include "AprilTags/Tag25h7.h"
#include "AprilTags/Tag25h9.h"
#include "AprilTags/Tag36h9.h"
#include "AprilTags/Tag36h11.h"
#include <cmath>

using namespace cv;
using namespace std;

Mat image;

bool selectObject = false;
int trackObject = 0;
Point origin;
Rect selection;
DJI_event g_event;
DJI_lock g_lock;
#define WIDTH 320
#define HEIGHT 240
#define IMAGE_SIZE (HEIGHT * WIDTH)


AprilTags::TagDetector* m_tagDetector;
AprilTags::TagCodes m_tagCodes = AprilTags::tagCodes36h11;




Mat g_imleft(HEIGHT, WIDTH, CV_8U);
Mat g_imright(HEIGHT, WIDTH, CV_8U);
Mat	g_depth(HEIGHT, WIDTH, CV_16SC1);
e_vbus_index selected_vbus = e_vbus1;  // select front vbus
string winDemoName = "Guidance Tracking Demo";

// utility function to provide current system time (used below in
// determining frame rate at which images are being processed)
double tic() {
  struct timeval t;
  gettimeofday(&t, NULL);
  return ((double)t.tv_sec + ((double)t.tv_usec)/1000000.);
}

bool Id2XY(int id,float &x , float &y)
{
    int ty=id/24;
    int tx=id%24;
    if (id<0 || id>102 ||x>6) return false;
    x=tx;
    y=ty;
    return true;
}

int my_callback( int data_type, int data_len, char *content )
{
	printf("enter callback..\n");
	g_lock.enter();
	if ( e_image == data_type && NULL != content )
	{
		printf("callback: type is image..\n");
		image_data data;
		memcpy((char*)&data, content, sizeof(data));
		memcpy(g_imleft.data, data.m_greyscale_image_left[selected_vbus], IMAGE_SIZE);
		memcpy(g_imright.data, data.m_greyscale_image_right[selected_vbus], IMAGE_SIZE);
		memcpy(g_depth.data, data.m_depth_image[selected_vbus], IMAGE_SIZE * 2);
	}
	g_lock.leave();
	g_event.set_event();
	return 0;
}


#define RETURN_IF_ERR(err_code) { if( err_code ){ printf( "USB error code:%d in file %s %d\n", err_code, __FILE__, __LINE__ );}}
#define RELEASE_IF_ERR(err_code) { if( err_code ){ release_transfer(); printf( "USB error code:%d in file %s %d\n", err_code, __FILE__, __LINE__ );}}

int main( int argc, const char** argv )
{

	VideoWriter	vWriter("result.avi", CV_FOURCC('M','J','P','G'), 25, Size(WIDTH, HEIGHT), false);
    Rect trackWindow;
	int hsize[] = {16, 16};
    float hranges[] = {0, 255};
	float dranges[] = {0, 255};

	const float* phranges[] = { hranges, dranges };

	selection = Rect(10,10,100,100);
	trackObject = 0;

    namedWindow( winDemoName, 0 );

    Mat imcolor, mask, hist, hist_of_object, backproj;
    bool paused = false;
	float hist_update_ratio = 0.2f;

	// Connect to Guidance and subscribe data
	reset_config();
	int err_code = init_transfer();
	RETURN_IF_ERR(err_code);

	err_code = select_greyscale_image(selected_vbus, true);
	RELEASE_IF_ERR(err_code);

	err_code = select_greyscale_image(selected_vbus, false);
	RELEASE_IF_ERR(err_code);

	err_code = select_depth_image(selected_vbus);
	RELEASE_IF_ERR(err_code);

	err_code = set_sdk_event_handler(my_callback);
	RELEASE_IF_ERR(err_code);

	err_code = start_transfer();
	RELEASE_IF_ERR(err_code);

	Mat depth(HEIGHT, WIDTH, CV_8UC1);
    {//init of april tags

    m_tagDetector = new AprilTags::TagDetector(m_tagCodes);

    }

    for( int times = 0; times < 30000; ++times )
    {
		g_event.wait_event();

		// filter depth image
		filterSpeckles(g_depth, -16, 50, 20);
		// convert 16 bit depth image to 8 bit
		//g_depth.convertTo(depth, CV_8UC1);

		//imshow("depth", depth);
		waitKey(1);
		g_imleft.copyTo(image);

        { //dealing with marks;


            // detect April tags (requires a gray scale image)
            double t0;
            t0 = tic();
            vector<AprilTags::TagDetection> detections = m_tagDetector->extractTags(image);
            double dt = tic()-t0;
            cout << "Extracting tags took " << dt << " seconds." << endl;

            // print out each detection
            cout << detections.size() << " tags detected:" << endl;

            for (int i=0; i<detections.size(); i++) {
                // also highlight in the image
                detections[i].draw(image);
            }
        }


        imshow( winDemoName, image );
		vWriter<<image;

        char c = (char)waitKey(10);
        if( c == 27 || c=='q')
		{
            break;
		}
        switch(c)
        {
        case 'c':
            trackObject = 0;
            break;
        case 'p':
		case ' ':
            paused = !paused;
            break;
        default:
            ;
        }
    }

	err_code = stop_transfer();
	RELEASE_IF_ERR(err_code);

	sleep(100000);

	err_code = release_transfer();
	RETURN_IF_ERR(err_code);

    return 0;
}
